#include "User_Program.h"
#include "Ano_ProgramCtrl_User.h"
#include "Drv_OpenMV.h"

#include "Drv_usart.h"
#include "Drv_led.h"
#include "Ano_DT.h"
#include "Ano_T265.h"

//////
#include "Ano_Imu.h"
#include "Ano_Imu_Data.h"
#include "Ano_Pid.h"

#include "Ano_FlightCtrl.h"
/////////////////

uint8_t Input_mode_flag=0;//输入模式标志位
uint8_t Input_mode_position_1_flag=0;//模式1送货位置1上升标志位
uint8_t Input_mode_position_2_flag=0;//模式1送货位置2上升标志位
uint8_t Input_mode_2_position_1_flag=0;//模式2送货位置1上升标志位
uint8_t Input_mode_2_position_2_flag=0;//模式2送货位置2上升标志位

Point_t Input_mode_position_1;//模式1送货位置1
Point_t Input_mode_position_2;//模式1送货位置2

uint8_t Input_mode_2_Learning_color_blocks;//学习的色块
Point_t Input_mode_2_position_1;//模式2送货位置1
Point_t Input_mode_2_position_2;//模式2送货位置2

u8 Input_mode_position_Str[100];//接收字符串
u8 opencv_Get_Byte_Str[100];//opencv接收字符
u8 Input_mode_position_send_Str[50];//发送数据字符串

/////opencv学习指令以及opencv视觉定位
uint8_t Opencv_lenrning[5]   ={0xAA,0XFF,0X40,0X01,0X02};
uint8_t Opencv_positioning[5]={0xAA,0XFF,0X40,0X01,0X03};
uint8_t IF_Learning_color_blocks_flag;//选择定位还是学习
uint8_t IF_Reach_the_target_area_flag;//是否到达定位区域
///////////////////


//用户任务顶层


//uint8_t Flag;
///*
//        |+X 
//        |
//        |
//+Y------------- -Y
//        |
//        |
//        |-X
//        
uint8_t takeoff_flag;
FollowManager_t FollowManager;

extern _judge_sync_data_st jsdata;
///////////////航点列表/////////////
Point_t A0;
Point_t A1;
Point_t A2;
Point_t A3;
Point_t A4;
Point_t A5;
Point_t A6;
Point_t A7;
Point_t A8;
Point_t A9;
Point_t A10;
Point_t A11;




///////////////航点列表////////////
void Position_Init()
{
A0.x=42;
A0.y=117;
A0.z=110;
A0.code=0x09;

A1.x=40;
A1.y=245;
A1.z=110;
A1.code=0x01;

A2.x=108;
A2.y=180;
A2.z=110;
A2.code=0x08;

A3.x=110;
A3.y=49;
A3.z=110;
A3.code=0x07;

A4.x=178;
A4.y=-18;
A4.z=110;
A4.code=0x0A;

A5.x=176;
A5.y=116;
A5.z=110;
A5.code=0x02;

A6.x=172;
A6.y=249;
A6.z=110;
A6.code=0x0B;

A7.x=242;
A7.y=180;
A7.z=110;
A7.code=0x03;

A8.x=244;
A8.y=50;
A8.z=110;
A8.code=0x06;

A9.x=313;
A9.y=-18;
A9.z=110;
A9.code=0x04;

A10.x=310;
A10.y=117;
A10.z=110;
A10.code=0x0C;

A11.x=308;
A11.y=249;
A11.z=110;
A11.code=0x05;
}
///////////////////////////////////////
//串口2
//读取数据返回给辅控板
//参数：模式，数据位（第几个数据），返回值
//模式1接收数据1返回：   0xAA 0x09 0x01 0x01 0x01 0xFF
//模式1接收数据2返回：   0xAA 0x09 0x01 0x02 0x01 0xFF
//模式1没有接收数据1返回：   0xAA 0x09 0x01 0x01 0x00 0xFF
//模式1没有接收数据2返回：   0xAA 0x09 0x01 0x02 0x00 0xFF
//模式2接收数据返回：        0xAA 0x09 0x02 0x00 0x01 0xFF
//模式2没有接收色块返回：    0xAA 0x09 0x02 0x00 0x00 0xFF
void Input_mode_send(u8 mode_select,u8 Data_bit_selection,u8 mode_state)
{
	u8 _cnt = 0;
	
	Input_mode_position_send_Str[_cnt++] = 0xAA;
	Input_mode_position_send_Str[_cnt++] = 0x09;
	Input_mode_position_send_Str[_cnt++] = mode_select;
	Input_mode_position_send_Str[_cnt++] = Data_bit_selection;
	Input_mode_position_send_Str[_cnt++] = mode_state;
	Input_mode_position_send_Str[_cnt++] = 0xFF;

	Usart2_Send(Input_mode_position_send_Str,_cnt);
}



//*******动作串口同样使用串口2******
//同样使用字符串Input_mode_position_send_Str
//吊舱打开(一套)：0xAA 0x01 0x02 0xFF
//激光5打开	0xAA 0x05 0x01 0xFF
//激光5关闭	0xAA 0x05 0x02 0xFF
void Action_mode_send(u8 mode_select,u8 mode_state)
{
	u8 _cnt = 0;
	
	Input_mode_position_send_Str[_cnt++] = 0xAA;
	Input_mode_position_send_Str[_cnt++] = mode_select;
	Input_mode_position_send_Str[_cnt++] = mode_state;
	Input_mode_position_send_Str[_cnt++] = 0xFF;

	Usart2_Send(Input_mode_position_send_Str,_cnt);
}




//模式设置（串口2接收数据）
//定义各个模式操作
//模式1数据构成:帧头0xAA,模式0x01,点的顺序(0x01)(0x02),点坐标(12种),帧尾0xFF
//模式2数据构成:帧头0xAA,模式0x02,选项(0x01)(0x02),0x01,帧尾0xFF
void Input_mode_position_Get_Byte(u8 data)
{
	
	static u8 Input_mode_sta = 0;
	static u8 Input_mode_cnt=1;
	
	if(Input_mode_sta==0&&data==0xAA)	//帧头0xAA
	{
		Input_mode_sta=1;
		Input_mode_position_Str[0]=data;
	}
	
	else if(Input_mode_sta==1&&data==0x01)	//模式1地址，0x01
	{
		Input_mode_sta=21;//表示第2个数据选择模式1
		Input_mode_position_Str[Input_mode_cnt]=data;
		Input_mode_cnt++;
	}
	else if(Input_mode_sta==1&&data==0x02)	//模式2地址，0x02
	{
		Input_mode_sta=22;//表示第2个数据选择模式2
		Input_mode_position_Str[Input_mode_cnt]=data;
		Input_mode_cnt++;
	}
	
	
    //*************模式1主要数据接收操作******************
	else if(Input_mode_sta==21)		//n份数据
	{
		//接收数据
		Input_mode_position_Str[Input_mode_cnt]=data;
		Input_mode_cnt++;
		
		if(Input_mode_cnt==4)//Input_mode_cnt进入为2，接收2个数据
      {
			  Input_mode_sta=31;//进入判断帧尾操作，表示3阶段是模式1操作
				Input_mode_cnt=1;//重置操作
			}
	}
	else if(Input_mode_sta==31&&data==0xFF)		//帧尾
	{
		//主要操作
		Input_mode_flag=1;//选择模式1
		
		if(Input_mode_position_Str[2]==0x01)//进行第一个点的设置坐标
		{
			if(Input_mode_position_Str[3]==0x01) Input_mode_position_1=A1;
		  else if(Input_mode_position_Str[3]==0x02) Input_mode_position_1=A5;
			else if(Input_mode_position_Str[3]==0x03) Input_mode_position_1=A7;
			else if(Input_mode_position_Str[3]==0x04) Input_mode_position_1=A9;
			else if(Input_mode_position_Str[3]==0x05) Input_mode_position_1=A11;
			else if(Input_mode_position_Str[3]==0x06) Input_mode_position_1=A8;
			else if(Input_mode_position_Str[3]==0x07) Input_mode_position_1=A3;
			else if(Input_mode_position_Str[3]==0x08) Input_mode_position_1=A2;
			else if(Input_mode_position_Str[3]==0x09) Input_mode_position_1=A0;
			else if(Input_mode_position_Str[3]==0x0A) Input_mode_position_1=A4;
			else if(Input_mode_position_Str[3]==0x0B) Input_mode_position_1=A6;
			else if(Input_mode_position_Str[3]==0x0C) Input_mode_position_1=A10;
      else 
			{
				Input_mode_send(1,1,0);//模式1，数据位1，（成功为1，失败为0）
			}				
	    
			//发送数据字符串
			Input_mode_send(1,1,1);//模式1，数据位1，（成功为1，失败为0）
		}
		else if(Input_mode_position_Str[2]==0x02)//进行第一个点的设置坐标
		{
			if(Input_mode_position_Str[3]==0x01) Input_mode_position_2=A1;
		  else if(Input_mode_position_Str[3]==0x02) Input_mode_position_2=A5;
			else if(Input_mode_position_Str[3]==0x03) Input_mode_position_2=A7;
			else if(Input_mode_position_Str[3]==0x04) Input_mode_position_2=A9;
			else if(Input_mode_position_Str[3]==0x05) Input_mode_position_2=A11;
			else if(Input_mode_position_Str[3]==0x06) Input_mode_position_2=A8;
			else if(Input_mode_position_Str[3]==0x07) Input_mode_position_2=A3;
			else if(Input_mode_position_Str[3]==0x08) Input_mode_position_2=A2;
			else if(Input_mode_position_Str[3]==0x09) Input_mode_position_2=A0;
			else if(Input_mode_position_Str[3]==0x0A) Input_mode_position_2=A4;
			else if(Input_mode_position_Str[3]==0x0B) Input_mode_position_2=A6;
			else if(Input_mode_position_Str[3]==0x0C) Input_mode_position_2=A10;
			else 
			{
				Input_mode_send(1,2,0);//模式1，数据位1，（成功为1，失败为0）
			}				

      Input_mode_send(1,2,1);//模式1，数据位2，（成功为1，失败为0）
		}	

		
	}
	       //***************************************************
	
	
	
	        //*************模式2主要数据接收操作******************
	
	//模式3确认按下：0xAA,0x02,0x01（！）,0x01,0xFF
	//模式3色块选择确认按下：0xAA,0x02,0x02（！）,0x01,0xFF

	
	//模式2确认按下
	  else if(Input_mode_sta==22)		
	  {
		//接收数据
		Input_mode_position_Str[Input_mode_cnt]=data;
		Input_mode_cnt++;
		
		if(Input_mode_cnt==4)//Input_mode_cnt进入为2，接收2个数据
      {
			  Input_mode_sta=32;//进入判断帧尾操作，表示3阶段是模式2操作
				Input_mode_cnt=1;//重置操作
			}
	  }
	  else if(Input_mode_sta==32&&data==0xFF)		//帧尾
	  {
			//主要操作
				if(Input_mode_position_Str[2]==0x01&&Input_mode_position_Str[3]==0x01)
				{		
					Input_mode_flag=2;//选择模式2并且开始写入色块
					IF_Learning_color_blocks_flag=1;					
				}				
				else if(Input_mode_position_Str[2]==0x02&&Input_mode_position_Str[3]==0x01)//选择模式2写入色块确认按键
				{	
					IF_Learning_color_blocks_flag=0;
				}	
				else
				{
					Input_mode_send(2,0,0);//代表模式2接收失败，0:无模式
				}				
		}

	        //***************************************************
	else
	{
		Input_mode_sta = 0;
		//Input_mode_send(0,0,0);
		
	}
}
	
///发送开始学习信息给上位机 10ms
	void sand_IF_Learning_color_blocks()
{	
	if(IF_Learning_color_blocks_flag==1)//选择模式中
	{
		Usart3_Send(Opencv_lenrning,5);//串口3发送通知上位机开启学习模式		
		Input_mode_2_Learning_color_blocks=Color_block_input_id();
		//学习的色块信息  1：红色正方形  2：红色三角形  3：红色圆形
		//                4：蓝色正方形  5：蓝色三角形  6：蓝色圆形  
				if(Input_mode_2_Learning_color_blocks==1&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A0;
					Input_mode_2_position_2=A4;
					
					Input_mode_send(2,1,1);//代表模式2接收1色块成功，
				}
				else if(Input_mode_2_Learning_color_blocks==2&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A1;
					Input_mode_2_position_2=A5;
					
					Input_mode_send(2,2,1);//代表模式2接收2色块成功，
				}
				
				else if(Input_mode_2_Learning_color_blocks==3&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A8;
					Input_mode_2_position_2=A11;
					
					Input_mode_send(2,3,1);//代表模式2接收3色块成功，
				}
				
				else if(Input_mode_2_Learning_color_blocks==4&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A6;
					Input_mode_2_position_2=A10;
					
					Input_mode_send(2,4,1);//代表模式2接收4色块成功，
				}
				
				else if(Input_mode_2_Learning_color_blocks==5&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A7;
					Input_mode_2_position_2=A9;
					
					Input_mode_send(2,5,1);//代表模式2接收5色块成功，
				}
				
				else if(Input_mode_2_Learning_color_blocks==6&&Input_mode_flag==2)
				{
					Input_mode_2_position_1=A3;
					Input_mode_2_position_2=A2;
					
					Input_mode_send(2,6,1);//代表模式2接收6色块成功，
				}
				
				else
				{
					Input_mode_send(2,0,0);//代表模式2接收失败，
				}
				
  }
	
	else if(IF_Learning_color_blocks_flag==0)
	{
		Usart3_Send(Opencv_positioning,5);//串口3发送通知上位机色块定位模式 
	}
	
}



//bool Enter = true;
//当调用该函数时，维持当前动作 HoldTime 毫秒（或单位时间）
//时间到后，切换到下一个动作 NextAction
bool ActionHoldChange(uint16_t HoldTime, FSMList_t NextAction)
{
	static bool Enter = true; //表示第一次进入
    static uint16_t CountDown = 0;

    if (Enter)//如果是第一次进入
    {
        CountDown = HoldTime;
        Enter = false;   //删除第一次进入标志
    }
    else
    {
        CountDown--;
    }

    if (CountDown == 0)  //计数完成
    {
        Enter = true;
        FollowManager.ActionList = NextAction;//执行下一次动作
			return 1;
    }
		return 0;
}



//动作保持函数
bool ActionDelay(int16_t HoldTime)
{
    static bool Enter_Delay = true;
    static uint16_t CountDown_Delay = 0;

    if (Enter_Delay)
    {
        CountDown_Delay = HoldTime;
        Enter_Delay = false;
    }
    else
    {
        CountDown_Delay--;
    }

    if (CountDown_Delay == 0)
    {
        Enter_Delay = true;
		return 1;
    }
		return 0;

}



//到达指定x,y,z坐标
bool Go_Position_XYZ(Point_t Pos)
{
	
	if(sdk_pos_set(Pos.x,Pos.y)&&sdk_alititude_set(Pos.z))
  {
		return 1;
	}
    return 0;
}




//到达指定x,y坐标
bool Go_Position_XY(Point_t Pos)
{
	if(sdk_pos_set(Pos.x,Pos.y))
{
	return 1;
}
    return 0;
}



//FSMList_t:动作选择列表//FollowManager：动作执行列表
void Goto_Action(FSMList_t GoAction)
{
	FollowManager.ActionList=GoAction;//表示执行GoAction动作
}


uint8_t ActionIRQ_Flag=0;//任务中断标志位

/*任务中断请求接口函数 */
void ActionIRQ_Request(FSMList_t Action_Req)
{
ActionIRQ_Flag=1;
FollowManager.Action_IRQ=Action_Req;
}



/*任务中断请求处理Handler */
//判断有无任务进入
void SoftActionIRQ()
{
	if(ActionIRQ_Flag)//当有中断请求标志位时
	{
    FollowManager.Action_Last= FollowManager.ActionList;//Action_Last将ActionList先存储起来，此时ActionList中的任务被打断
		ActionIRQ_Flag=0;
    FollowManager.ActionList= FollowManager.Action_IRQ;//切换为Action_IRQ中断任务执行
	}
	
}

/*任务中断退出恢复原任务函数*/
void Action_Resume()
{
	FollowManager.ActionList= FollowManager.Action_Last;//恢复原来被打断的任务
}


//////////////////////////////////////////////////
u8 pid_data[6];
extern _PID_val_st PID_X_Val;
extern _PID_val_st PID_Y_Val;
extern _PID_val_st PID_H_Val;


//10ms一次循环 100即为1s
void UpdateStatus(float dT_s,s16 *CH_N)
{
	
	//发送滤波过后的设定坐标数据
	filtered_set_coordinate_data();
  
	pid_data[0]=((int16_t)(PID_X_Val.err)&0xFF);
	pid_data[1]=(int16_t)(PID_X_Val.err)>>8;
	pid_data[2]=100;
	SendCustomData(USE_U2, pid_data,6);
	sand_IF_Learning_color_blocks();//模式2发送if学习状态还是定位状态
	//////////////测试代码////////////////
   
	
	// 按键下拨后判断按键 急停操作
  if(takeoff_flag==1)
	{
		if(CH_N[7]<-400)//通道7再次上拨，下降操作
		{
		  sdk_land();
			Goto_Action(ActionWaitting);
			takeoff_flag=2;
    }
  }
	
	
  //判断按键下拨 0.1秒后起飞 进入程控模式
	//takeoff_flag初始值为0
	if(takeoff_flag==0)
	{
		if(CH_N[7]>400)//通道7下拨
		 {
//       if(Input_mode_flag != 0)	//判断是否选择任务，没有则不执行任务
//			 {				 
			 Goto_Action(ActionCountdown); 
		   takeoff_flag=1;
//			 }
		 }
	}
	
	
	//定位模式且到达目标区域
  if(IF_Learning_color_blocks_flag==0&&IF_Reach_the_target_area_flag==1)
  { 
		ActionIRQ_Request(Action_CV_Point);
  }
	

SoftActionIRQ();//任务中断线程

//开始执行任务函数
switch(FollowManager.ActionList)
{
		case(ActionWaitting):
	{		
		break;
	}

	
	case(ActionCountdown):
	{
		ledFlash(0.01,BIT_RLED,40,40);//调用使红色 LED 快速闪烁，频率约为每秒 25 次
																	//其中40 40 为高电平和低电平的持续时间（单位可能是 ms）			
		flag.action_activated=1;      //表示当前正在执行程控动作（后续视觉识别定位时需要退出程控模式）
		if(ActionHoldChange(10,ActionTakeOff))
		{		
			AnoDTSendStr(USE_U2,0xA0,1,"TakeOFF!!");//等待 10 个 tick 或毫秒 后切换到下一个动作 ActionTakeOff
		}
			break;
	}
	
	case(ActionTakeOff):
	{
			//一键起飞
		  
		  sdk_takeoff(130);//飞机高度20cm，实际飞行高度为110+20cm
		  Goto_Action(ActionHoverStartPoint);
		 
			break;
	}
	
	case ActionHoverStartPoint:
	{
		sdk_pos_set(0,0);
		
		if(Input_mode_flag==0)//无模式状态
		{
		  ActionHoldChange(1200,Action_A0);
			break;
		}
		
		if(Input_mode_flag==1)//模式1
		{
		  ActionHoldChange(1200,Action_Input_mode_position_1);			
			break;
		}
		
		if(Input_mode_flag==2&&Input_mode_2_Learning_color_blocks !=0)//模式2，并且已经输入色块
		{
		  ActionHoldChange(1200,Action_Input_mode_2_position_1);			
			break;
		}
		
	}
	
	case Action_A0:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Program_Ctrl_User_Set_HXYcmps(10.0,10.0);// 10cm/s
			Go_Position_XY(A0);
			ActionHoldChange(800,Action_A1);
			break;
		}

	}
	
	
	case Action_A1:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A1);
			ActionHoldChange(800,Action_A2);
			break;
		}
	}
	
	
	case Action_A2:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A2);
			ActionHoldChange(800,Action_A3);
			break;
		}
	}
	
	
	case Action_A3:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A3);
			ActionHoldChange(800,Action_A4);
			break;
		}
	}
	
	case Action_A4:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A4);		
			ActionHoldChange(800,Action_A5);
			break;
		}
	}
	
	case Action_A5:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A5);
			ActionHoldChange(800,Action_A6);
			break;
		}
	}
	
	case Action_A6:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A6);
			ActionHoldChange(800,Action_A7);		
			break;
		}
	}
	
	case Action_A7:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A7);
			ActionHoldChange(800,Action_A8);
			break;
		}
		
	}
	
	case Action_A8:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A8);
			ActionHoldChange(800,Action_A9);
			break;
		}
		
	}
	
	case Action_A9:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A9);
			ActionHoldChange(800,Action_A10);		
			break;
		}
	}
	
	case Action_A10:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A10);
			ActionHoldChange(800,Action_A11);		
			break;
		}
	}
	
	case Action_A11:
	{
		if(Input_mode_flag==0)//无模式状态
		{
			Go_Position_XY(A11);
			ActionHoldChange(800,Action_A12);//到达最后一个点后，下一步回到原点			
			break;
		}
	}
	

	case Action_A12://回到原点操作
	{
		Program_Ctrl_User_Set_HXYcmps(10.0,10.0);// 10cm/s
		sdk_pos_set(0,0);	
		ActionHoldChange(1500,Action_Land);
		
		Input_mode_position_1_flag=0;
		Input_mode_position_2_flag=0;
		break;
	}


	case(Action_Land):
	{
		if(sdk_pos_set(0,0))
		{
			Program_Ctrl_User_Set_Zcmps(-5);//下降速度8cm/s			
      AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"downing!!");//发送数据给上位机					
			if(jsdata.of_alt<=40)
			{				
				sdk_land();
				Goto_Action(ActionWaitting);			
			}
		}
		break;
	}
	

   case(Action_Input_mode_position_1)://模式1位置1
	 {		
		Go_Position_XY(Input_mode_position_1);
		ActionHoldChange(1500,Action_Z_80CM);		
    Input_mode_position_1_flag=1;
		Input_mode_position_2_flag=0;
		break;
	 }
	
	case(Action_Input_mode_position_2)://模式1位置2
	{		
		Go_Position_XY(Input_mode_position_2);
		ActionHoldChange(1500,Action_Z_80CM);		
    Input_mode_position_1_flag=0;
		Input_mode_position_2_flag=1;
		break;
	}

	case(Action_Input_mode_2_position_1)://模式2位置1
	 {		
		Go_Position_XY(Input_mode_2_position_1);
		ActionHoldChange(1500,Action_Z_80CM);		
    Input_mode_2_position_1_flag=1;
		Input_mode_2_position_2_flag=0;
		break;
	 }
	
	 
	 case(Action_Input_mode_2_position_2)://模式2位置2
	 {		
		Go_Position_XY(Input_mode_2_position_2);
		ActionHoldChange(1500,Action_Z_80CM);		
    Input_mode_2_position_1_flag=0;
		Input_mode_2_position_2_flag=1;
		break;
	 }
	 
	
	case(Action_Z_80CM):
	{
		Program_Ctrl_User_Set_HXYcmps(0,0);	
		if( sdk_alititude_set(70) )//到达80cm处
		{
			IF_Reach_the_target_area_flag=1;//表示到达80cm处，可以开始定位		
		}
		break;
	}
	
	
	case(Action_Z_150CM):
	{		
		if(sdk_alititude_set(130)==1 &&Input_mode_position_1_flag==1)
		{
			ActionHoldChange(500,Action_Input_mode_position_2);//到达模式1位置2
		}
		
		if(sdk_alititude_set(130)==1 && Input_mode_position_2_flag==1)//到达原点处
		{
			ActionHoldChange(500,Action_A12);//回到原点
		}
		
		if(sdk_alititude_set(130)==1 && Input_mode_2_position_1_flag==1)//到达模式2位置2
		{
			ActionHoldChange(500,Action_Input_mode_2_position_2);//回到原点
		}
		
		if(sdk_alititude_set(130)==1 && Input_mode_2_position_2_flag==1)//到达原点处
		{
			ActionHoldChange(500,Action_A12);//回到原点
		}
		
		break;
	}
	
	
	
		case(Action_CV_Point)://80cm处自动定位，2s后执行动作
		{
			//这里同时下放吊舱			
			OpenMV_P_ctrl();
			
			static bool Enter_opmv = true;
			static uint8_t sta = 0;
			
			if(sta==0)//持续两秒后在开启播报语音//激光5打开
			{
				if(ActionDelay(200))
				sta=1;	
			}
			else if(Enter_opmv == true&& sta==1) //语音只在开始时执行一次
			{
					Action_mode_send(0x05,0x01);//激光5打开	0xAA 0x05 0x01 0xFF   
					Action_mode_send(0x0A,0x01);//语音提示收货	0xAA 0x05 0x01 0xFF	
					sta=2;
					Enter_opmv=false;
			}
			else if(sta==2)//倒计时结束后
			{
				if(ActionHoldChange(500,Action_Z_150CM))
					{
					 Enter_opmv=true;
					 sta=0;
					 IF_Reach_the_target_area_flag=0;//定位模式关闭
					 Action_mode_send(0x05,0x02);//激光5关闭	0xAA 0x05 0x02 0xFF
					 //这里收回放吊舱			
			     }				
			}
				
			break;
		}
		
		
		
	///////////////////////////////////////////////////////////////////
 }

}


